#pragma once

#include "PID.h"
#include "eepromManager.h"
#include "WP.h"
#include <StandardCplusplus.h>
#include <vector>

class Aircraft;

class Ap
{
private:
	bool en;
	int mode, indv_holds;
	long lastPidTime;
	Aircraft *aircraft;
	
	struct { // 7 in total
		double roll,heading,pitch,vrate,altitude,speed,slip;
	} pidTarget;
	
	double fcs_roll,fcs_pitch,fcs_throttle,fcs_yaw;
	
	std::vector<WP> wp;
	WP *trackingWp;
	
	void readInPID(PID &pid, PID::Spid &structPid);
	void onRun(double dt);
	void offRun();
	double findDT();
	
	void runWpNav(double dt);
	void runLand(double dt);
	void runIndv(double dt);
	
	int findWpIndex(unsigned int id); // -1 for error
	
public:
	Ap();
	bool isEn();
	void init(Aircraft *aircraft);
	void run();
	
	struct { // 7 in total
		PID rollHold,headingHold,pitchHold,vrateHold,altHold,throttleHold,slipHold;
	} pid;
	
	//Settings Handlers {
	bool switchToMode(int mode);
	bool switchIndvMode(int mode);
	int getMode();
	int getIndvMode();
	void indv_roll();
	void indv_heading();
	void indv_rollOff();
	void indv_pitch();
	void indv_vrate();
	void indv_alt();
	void indv_pitchOff();
	void indv_thro();
	void indv_throOff();
	void indv_slip();
	void indv_slipOff();
	
	bool setTargetRoll(double roll);
	bool setTargetHeading(double heading);
	bool setTargetPitch(double pitch);
	bool setTargetVRate(double vrate);
	bool setTargetAlt(double alt);
	bool setTargetThrottle(double throttle);
	bool setTargetSlip(double slip);
	
	double getTargetRoll();
	double getTargetHeading();
	double getTargetPitch();
	double getTargetVRate();
	double getTargetAlt();
	double getTargetThrottle();
	double getTargetSlip();
	
	void savePIDtoEEPROM();
	
	bool addWp(WP newWp);
	bool addWp(double lat, double lon, double alt);
	bool rmWpId(unsigned int id);
	bool mvWpIdUnder(unsigned int idFrom,unsigned int idTo);
	bool mvWpIdOver(unsigned int idFrom,unsigned int idTo);
	bool rmWp(unsigned int index);
	bool mvWp(unsigned int indexFrom, unsigned int indexTo);
	std::vector<WP> getWpList();
	//} Settings Handlers
};
